#include "robot.h"
#include "astar.h"

Robot robots[ROBOT_NUM];

extern char map[MAP_SIZE][MAP_SIZE];
extern int current_frame;
extern Berth berths[BERTH_NUM];
extern unordered_set<Cargo, CargoHash> CargoSet; // 货物集

// 将Robot路径设为离所属港口最近的货物
void Robot::SetRouteToCargo(){
    route.clear(); // 清除旧的route
    Cargo nearest = *(belonging->cargo_set.begin()); 
    route = FindRoute(p, nearest.p, map);
    while(CargoSet.find(Cargo(nearest.p, nearest.val, nearest.frame)) == CargoSet.end() ||route.size() == 0 || nearest.frame + CARGO_TIMESPAN < current_frame + route.size() + CARGO_WASTE_TIME){ // 已经没了||没有路径到达该点||来不及了
        belonging->cargo_set.erase(nearest);
        if(belonging->cargo_set.size() == 0) break;
        nearest = *(belonging->cargo_set.begin()); 
        route = FindRoute(p, nearest.p, map);
    }
    
    if(route.size() != 0){
        target = nearest.p;
        // 宣示主权，避免机器人们抢货
        belonging->cargo_set.erase(nearest);
        CargoSet.erase(Cargo(nearest.p, nearest.val, nearest.frame));
    }else{
        target.x = -1;
    }
}

// 机器人是否到港口了
bool Robot::ArriveAtBerth(){
    return (cargo == 1 && p.x >= belonging->p.x && p.y >= belonging->p.y && p.x <= belonging->p.x + 3 && p.y <= belonging->p.y + 3);
}

// 机器人交货
void Robot::Deliver(int i){
    if(cargo == 1){
        printf("pull %d\n", i); // 交货
        belonging->currentcargo++;
        belonging->totalcargo++;
    }else{
        belonging->misscargo++;
    }

    if(belonging->cargo_set.size()  > 0){
        SetRouteToCargo();
    }else{ // 暂时无货可拿(好像不太可能...)
        target.x = -1;
        route.clear();
    }
}

// 将Robot重新绑定到最近港口
void Robot::ReBindBerth(){
    int nearestBeth = -1;
    int nearest = MAX_DISTANCE;
    for(int i = 0;i < BERTH_NUM;i++){
        if(berths[i].close) continue;
        list<Point> route = (FindRoute(p, berths[i].p, map));
        if(route.size() > 0 && route.size() < nearest){
            nearest = route.size();
            nearestBeth = i;
        }
    }
    if(nearestBeth != -1){
        belonging = &berths[nearestBeth];
    }else{
        belonging = nullptr; // 无港口可去
    }
}

// 将Robot路径设为所属港口
void Robot::SetRouteToBerth(){
    route.clear(); // 清除旧的route
    target = belonging->p;
    route = FindRoute(p, target, map);
}

// 重新设置路径到target
void Robot::ResetRoute(){
    route.clear(); // 清除旧的route
    route = FindRoute(p, target, map);
}

// 避撞
void Robot::AvoidCollision(int i, Point &next, bool &retreat){
    bool collision[ROBOT_NUM] = {false};  // 指示即将会和哪些机器人撞上
    char replace[ROBOT_NUM];

    for(int j = i + 1;j < ROBOT_NUM;j++){
        if(robots[j].dead > 0) continue;
        if(p.Distance(robots[j].p) <= 3){ // 2的话三个机器人碰上有可能卡死，我也不确定改成3好不好。。。
            collision[j] = true;
            replace[j] = map[robots[j].p.x][robots[j].p.y];
            map[robots[j].p.x][robots[j].p.y] = '#'; // 将机器人当作障碍
        }
    }
    ResetRoute();
    if(route.size() == 0){ // 堵死了
        // 退一步
        retreat = true;
        if(pre[0] == Point(-1, -1)){
            //退两步都解决不了，去死吧
            fprintf(stderr, "robots[%d] at (%d %d) is dead...\n", i, robots[i].p.x, robots[i].p.y);
            dead = ROBOT_DEAD_TIME;
            map_restore = map[p.x][p.y];
            map[p.x][p.y] = '#';
        }else{
            next = pre[0];
            pre[0] = pre[1];
            pre[1] = Point(-1, -1);
        }
    }else{
        next = robots[i].route.front();
        robots[i].route.pop_front();
    }

    // 恢复地图
    for(int j = i + 1;j < ROBOT_NUM;j++){
        if(collision[j] == true){
            map[robots[j].p.x][robots[j].p.y] = replace[j];
        }
    }
}

// 移动到下一点: next
void Robot::Go(int i, const Point& next, bool retreat){
    // optimize n:如果跳帧这个pre就不准，这时候碰撞就G，比如map13就会
    if(!retreat){
        pre[1] = pre[0];
        pre[0] = p;
    }

    Point direction = next - p;
    int dir;
    // 往下为 X 轴正方向，往右为 Y 轴正方向。地图左上角坐标为原点(0,0)，右下角坐标为(199,199)。
    if(direction == Point(0, 1))       dir = 0; // 右
    else if(direction == Point(0, -1)) dir = 1; // 左
    else if(direction == Point(-1, 0)) dir = 2; // 上
    else if(direction == Point(1, 0))  dir = 3; // 下
    
    printf("move %d %d\n", i, dir);
}

// 检测一个点是否可以走，仅用在堵死的时候退避
bool CanGo(Point p){
    if (map[p.x][p.y] == '*' || map[p.x][p.y] == '#') return false;
    if (p.x < 0 || p.y < 0 || p.x >= MAP_SIZE || p.y >= MAP_SIZE) return false;

    return true;
}